/*
 * pgr_node.cpp
 *
 * Version: Feb. 29, 2012
 * Author:  Minghao Ruan
 * Note:    For SUMo project
 *
 */

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include "PgrUsbCam.h"
#include <unistd.h>     // for sleep(int second)

/* ROS Related Info:

@brief
      TODO:

@par Advertises
  -\b camera/image_raw topic (sensor_msgs/Image) raw 2D camera images
      (only raw if \b bayer_method is \b NONE).

  -\b 

@par Subscribes
  - NONE

@par Parameters
  -\b frame_id:
  -\b guid: @
  -\b

  -\b exposure: [int]
  -\b shutter:  [int]
  -\b whitebalance: not implemented
  -\b gain: [int]
  -\b brightness: [int]

*/

class StereoNode
{
private:
  // ROS related variables
  ros::NodeHandle nh;
  ros::NodeHandle pn;
  image_transport::ImageTransport it;

  PgrUsbCam* left;
  PgrUsbCam* right;

  // Initial camera hardware parameters
  int brightness;
  int exposure;
  double gain;
  double shutter;

  string frame_id;
  string left_info_url, right_info_url;
public:

  StereoNode(ros::NodeHandle n, ros::NodeHandle p)
    :nh(n),pn(p),it(n)
  {
    openCameras();

    ROS_INFO("Camera opened");
    configInitParams();
  }

  ~StereoNode()
  {
    delete left;
    delete right;
  }

  void openCameras(void)
  {
    PGRGuid guid;
    unsigned int rightID[] = {0xaca83ca8, 0xc5290948, 0xd249ee24, 0x654f7be4};
    unsigned int leftID[]= {0x9730f9fd, 0x0d7aaa35, 0x9948bbe2, 0xcd885362};

    std::string name;
   
    pn.param<string>("left_name", name, "left");

    //ROS_INFO("in open camera");
    memcpy(guid.value, leftID, 4*sizeof(unsigned int));
    left = new PgrUsbCam(&guid, name, nh, it);

    pn.param<string>("right_name", name, "right");
    memcpy(guid.value, rightID, 4*sizeof(unsigned int));
    right= new PgrUsbCam(&guid, name, nh, it);
  }

  void closeCamera(void)
  {
    left->stop();
    right->stop();
  }

  void configInitParams(void)
  {
    pn.param<int>("exposure", exposure, 58);
    pn.param<int>("brightness", brightness, 166);
    pn.param<double>("gain", gain, 0.0);
    pn.param<double>("shutter", shutter, 20.0);

    pn.param<string>("frame_id", frame_id, "camera_id");
    pn.param<string>("left/info_url", left_info_url, "");
    pn.param<string>("right/info_url",right_info_url, "");

 //   ROS_INFO("Configure left camera:");
 //   ROS_INFO("Exp:%d Br:%d G:%lf Sh:%lf",exposure,brightness,gain,shutter);
    left->configCamParams(exposure,brightness,gain,shutter);

 //   ROS_INFO("Configure right camera:");
 //   ROS_INFO("Exp:%d Br:%d G:%lf Sh:%lf",exposure,brightness,gain,shutter);
    right->configCamParams(exposure,brightness,gain,shutter);

    left->configCamInfo(frame_id, left_info_url);
    right->configCamInfo(frame_id, right_info_url);
  }

  void start()
  {
    left->start();
    right->start();
  }

#ifdef SOFTWARE_TRIGGER_CAMERA
  inline void fireSoftTrigger()
  {
    ros::Time t = ros::Time::now();
    left->SetTimeStamp(t);
    right->SetTimeStamp(t);
    left->FireSoftwareTrigger();
    right->FireSoftwareTrigger();
    // ROS_INFO("Trigger fired!");
  }

  inline void publishImage()
  {
    left->GrabnPublish();
    right->GrabnPublish();
  }
#endif
};

// Main entry point
int main(int argc, char* argv[])
{
  ros::init(argc, argv, "pgr_stereo");
  ros::NodeHandle nh, pn("~");

  ROS_WARN("start");
  StereoNode* sn = new StereoNode(nh, pn);

  sleep(2);
  ROS_INFO("Ready to go");
  sn->start();

#ifndef SOFTWARE_TRIGGER_CAMERA
  ros::spin();
#else
  double freq;
  pn.param<double>("freq", freq, 5);
  ROS_INFO("Software trigger");
  ros::Rate loop_rate(freq);
  while( nh.ok())
    {
      sn->fireSoftTrigger();
      sn->publishImage();
      ros::spinOnce();
      loop_rate.sleep();
    }
#endif

  //  sn->closeCamera();

  return 0;
}
